#include "rclcpp/rclcpp.hpp"
#include "robot_control/msg/lidar_data.hpp"
#include "std_msgs/msg/string.hpp"

#include <chrono>

using namespace std::chrono_literals;

class ObstacleDetector : public rclcpp::Node
{
public:
    ObstacleDetector() : Node("obstacle_detector")
    {
        // 声明参数
        this->declare_parameter("obstacle_threshold", 0.5);
        this->declare_parameter("publish_frequency", 5.0);
        
        // 获取参数
        obstacle_threshold_ = this->get_parameter("obstacle_threshold").as_double();
        double publish_frequency = this->get_parameter("publish_frequency").as_double();
        
        // 创建订阅者
        subscription_ = this->create_subscription<robot_control::msg::LidarData>(
            "/lidar_data", 10,
            std::bind(&ObstacleDetector::lidar_callback, this, std::placeholders::_1));
            
        // 创建发布者
        publisher_ = this->create_publisher<std_msgs::msg::String>("/obstacle_alert", 10);
        
        // 创建定时器用于定期发布状态
        auto timer_period = std::chrono::milliseconds(static_cast<int>(1000.0 / publish_frequency));
        status_timer_ = this->create_wall_timer(
            timer_period,
            std::bind(&ObstacleDetector::status_timer_callback, this));
            
        RCLCPP_INFO(this->get_logger(), "障碍物检测节点已启动，阈值: %.2f米", obstacle_threshold_);
    }

private:
    void lidar_callback(const robot_control::msg::LidarData::SharedPtr msg)
    {
        // 检查是否有障碍物
        bool obstacle_detected = false;
        int obstacle_direction = -1;
        
        for (int i = 0; i < 10; i++) {
            if (msg->distances[i] < obstacle_threshold_) {
                obstacle_detected = true;
                obstacle_direction = i;
                break;
            }
        }
        
        if (obstacle_detected) {
            last_obstacle_direction_ = obstacle_direction;
            last_obstacle_distance_ = msg->distances[obstacle_direction];
            has_obstacle_ = true;
        } else {
            has_obstacle_ = false;
        }
        
        // 更新最新数据时间
        last_data_time_ = this->now();
    }
    
    void status_timer_callback()
    {
        auto message = std_msgs::msg::String();
        
        if (has_obstacle_) {
            message.data = "警告！检测到障碍物在方向 " + 
                          std::to_string(last_obstacle_direction_) + 
                          "，距离: " + 
                          std::to_string(last_obstacle_distance_) + "米";
        } else {
            message.data = "安全：未检测到障碍物";
        }
        
        publisher_->publish(message);
        
        // 检查数据是否过期（超过1秒没有收到新数据）
        auto now = this->now();
        auto time_since_last_data = now - last_data_time_;
        if (time_since_last_data > rclcpp::Duration(1, 0)) {
            RCLCPP_WARN(this->get_logger(), "激光雷达数据接收超时");
        }
    }
    
    rclcpp::Subscription<robot_control::msg::LidarData>::SharedPtr subscription_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr status_timer_;
    double obstacle_threshold_;
    bool has_obstacle_ = false;
    int last_obstacle_direction_ = -1;
    float last_obstacle_distance_ = 0.0;
    rclcpp::Time last_data_time_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ObstacleDetector>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}